/*! \file	radiometry.hpp
 *  \brief	Declarations for the RADIOMETRY node.
*/

#ifndef _THERMALVIS_RADIOMETRY_H_
#define _THERMALVIS_RADIOMETRY_H_
	
#include "time.h"

#include <signal.h>

#include "ros_resources.hpp"
#include "tools.hpp"
#include "improc.hpp"

#include <dynamic_reconfigure/server.h>
#include "radiometryConfig.h"

// MODBUS STUFF
#include <inttypes.h>
#include <stdint.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <modbus.h>

const uint16_t UT_BITS_NB = 0x25;
const uint16_t UT_INPUT_BITS_NB = 0x16;
const uint16_t UT_REGISTERS_NB = 0x3;
const uint16_t UT_INPUT_REGISTERS_NB = 0x1;

const uint16_t ISOTECH_SETPOINT_ADDR = 0x18; // Address 24
const uint16_t ISOTECH_TEMP_ADDR = 0x01; // Address 1
const uint16_t ISOTECH_SP_ADDR = 0x02; // Address 1

// ---END MODBUS STUFF

const char __PROGRAM__[] = "RADIOMETRY";

#define DEFAULT_MIN_STABLE_TIME			60.0 	// in seconds
#define DEFAULT_MAX_STABLE_VARIATION	0.01	// in seconds
#define DEFAULT_SERIAL_POLLING_RATE 	0.1 	// in cycles per second
#define	DEFAULT_PORT_ADDRESS			"/dev/ttyUSB0"
#define DEFAULT_BAUD_RATE				9600
#define MAXIMUM_BLACKBODY_TEMPERATURE	80.0
#define TEMP_ERROR_HACK					0.001

typedef dynamic_reconfigure::Server < thermalvis::radiometryConfig > Server;

bool wantsToShutdown = false;
void mySigintHandler(int sig);

/// \brief		Stores configuration information for the radiometry node
struct radiometryData {
	
	string read_addr;
	
	string thermal_topic;
	
	string outputFolder;

	bool verboseMode;
	
	bool communicateWithBlackbody;
	double serialPollingRate;
	double minStableTime;
	double maxStableVariation;
	double targetTemperature;
	
	double completionTimeout;
	
	string portAddress;
	int baudRate;
	
	radiometryData();
	
	bool obtainStartingData(ros::NodeHandle& nh);   
	
};

/// \brief		Manages the driver / streamer
class radiometryNode {
private:

	char nodeName[256];
	
	bool targetTemperatureChanged;
	
	bool nodeStillValid;
	
	bool blackbodyIsSetup;
	
	char subdirectoryName[256];
	
	bool temperatureIsStable;
	bool stabilityOnceOffFlag;
	double stabilityTimer;
	double elapsedTime;
	ros::Time previousTime;
	
	int imageIndex;
	
	double currentTemp;
	
	cv::Mat lastFrame, frame;
	bool matEquality;
	
	string thermistorLogFile, imageLogFile, serialCommsFile;
	ofstream ofs_thermistor_log, ofs_image_log; //, ofs_serial_log;
	
	radiometryData configData;
	
	cv_bridge::CvImagePtr cv_ptr;
	
	image_transport::ImageTransport *it;
	image_transport::CameraSubscriber camera_sub;
	
	ros::Timer serial_timer;

	bool firstCall;
	
	// MODBUS STUFF
	modbus_t *ctx;
	uint8_t *tab_rp_bits;
	int nb_points;
	uint16_t *tab_rp_registers;
	
	
	// ---END MODBUS STUFF
	
	dynamic_reconfigure::Server<thermalvis::radiometryConfig> server;
	dynamic_reconfigure::Server<thermalvis::radiometryConfig>::CallbackType f;
	
	
public:
	
	radiometryNode(ros::NodeHandle& nh, radiometryData startupData);
	
	void handle_camera(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg);
	
	bool setupBlackbodyComms();
	bool setupBlackbody();
	
	bool changeSetPoint(double newVal);
	bool readTemperature(double& actualTemp);
	bool readSetpoint(double& setPoint);
	bool closeBlackbodyComms();
	
	void serialCallback(const ros::TimerEvent& e);
	
	void serverCallback(thermalvis::radiometryConfig &config, uint32_t level);
	
	void prepareForTermination();
	
	bool setupFilesAndDirectories();
	
	bool dataStillStreaming();
	
	bool isNodeValid();
	
	
	
};

boost::shared_ptr < radiometryNode > *globalNodePtr;

#endif
